#include "algorithm.hpp"
#include <math.h>


static const char *TAG = "Algorithm";



float pidController::update(float ref_val,float act_val, float dt)
{
    float error = ref_val - act_val;
    return update(error, dt);
}

float pidController::update(float error, float dt)
{
// 检查输入有效性
#if MATH_SIZE_OPTIMIZE
  if (dt <= 0)
  {
    LOGI(TAG, "dt is invalid");
    return 0;
  }
#else
  if (isnan(error))
  {
    LOGI(TAG, "error is nan");
    return 0;
  }
  else if (isinf(error))
  {
    LOGI(TAG, "error is inf");
    return 0;
  }
  else if (dt <= 0)
  {
    LOGI(TAG, "dt is invalid");
    return 0;
  }
#endif

  // === 比例项 ===
  float P = Kp * error;

  // === 积分项（梯形积分）===
  float integral_increment = 0.5f * (error + prev_error) * dt;
  integral += integral_increment;
  // LOGI(TAG,"the integral is %f", integral);
  float I = Ki * integral;

  // === 微分项（带低通滤波）===
  float derivative = 0.0f;
  if (dt > 1e-6f)
  { // 避免除以零
    derivative = (error - prev_error) / dt;
  }

  // 应用一阶低通滤波
  float alpha = dt / (tau + dt);
  float filtered_derivative =
      (1.0f - alpha) * prev_derivative + alpha * derivative;
  float D = Kd * filtered_derivative;

  // === PID 输出 ===
  float output = P + I + D;

  // === 输出限幅和抗积分饱和 ===
  if (fabsf(output) > max_output)
  {
    output = (output > 0) ? max_output : -max_output;
    // 抗饱和：回退当前步长的积分增量
    integral -= integral_increment;
  }

  // 更新状态
  prev_derivative = filtered_derivative;
  prev_error = error;

  return output;
}

void pidController::reset(float Kp, float Ki, float Kd, float max_output, float tau )
{
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
    this->max_output = max_output;
    this->tau = tau;
    integral = 0;
    prev_error = 0;
    prev_derivative = 0;
}